home *** CD-ROM | disk | FTP | other *** search
/ Aminet 7 / Aminet 7 - August 1995.iso / Aminet / misc / sci / RARS_Amiga_3.lha / RARS / cntrlr.cpp < prev    next >
Encoding:
C/C++ Source or Header  |  1995-05-27  |  10.5 KB  |  225 lines
  1. // CNTRLR.CPP - "driver" function for RARS - M. Timin, March, 1995
  2. // adapted to ver. 0.4 3/12/95 by M. Timin
  3. // for ver. 0.60
  4.  
  5.              /* Commented Robot Driver */
  6. /*
  7. This robot driver calculates a cornering speed for each corner based on its
  8. radius.  He accelerates on each straightaway until a "redline" speed
  9. is reached.  The "redline" value is calculated based on the cornering speed
  10. for the coming corner, and the distance remaining in the straight.
  11. Then he slows down, attempting to arrive at the corner with the proper
  12. cornering speed.  In the corner he attempts to maintain the cornering speed
  13. while attempting to stay near the inside.  His strategy for passing is to
  14. choose right or left at random, and then throw the car into a sharp slide
  15. toward that direction.
  16. */
  17.  
  18. #include <string.h>
  19. #include <stdlib.h>
  20. #include <math.h>
  21. #include "car.h"
  22.  
  23. // This structure will be built in the data area provided by the caller:
  24. struct params {   
  25.    double CORN_SPD_CON;   // determines how fast to take corners
  26.    double STEER_GAIN;     // servo gain, for staying in "lane"
  27.    double STEER_DAMP;     // servo damping, to prevent "weaving"
  28.    double BRAK_ACCEL; // we accelerate until this fraction of length
  29.    double END_CORNER;// used to decide when to start leaving the corer
  30.    double SLIP_LIM;  // maximum wheel slip, ft/sec, in wheel_slip()
  31.    double SLIP_CON;
  32.    double BIAS;
  33.    double NEAR_END;
  34.    double SHARP_TURN; // change in alpha when attempting to pass
  35.    int PASSING_TIME;  // time to stay in passing maneuver, counts
  36. };
  37.  
  38. inline double ABS(double arg) { return arg < 0.0 ? -arg : arg; }
  39.  
  40. // The following function calculates the speed for a corner.
  41. // The lateral force produced by cornering is proportional to the square
  42. // of the speed, and is inversely proportional to the radius of the path.
  43. // Therefore, the attainable cornering speed for differnt radii is
  44. // proportional to the square root of the radius.  This function implements
  45. // that rule.  The value to use for CORN_SPD_CON can be determined by
  46. // trial and error.  Example result: if the car is following a path with
  47. // radius of 100 ft, and if CORN_SPD_CON is 5.0, then corn_speed is 50 ft/sec.
  48. double corn_speed(double radius, double param)
  49. {
  50.    if(radius < 0.0)         // change sign of negative radius
  51.       radius = -radius;
  52.    else if(radius == 0.0)   // This is just insurance, this funtion doesn't
  53.       return(200.0);        // make sense when the radius is zero.
  54.    return param * sqrt(radius+45.0);
  55. }
  56.  
  57. // In order to set vc, if you know how fast you want to go (goal), and how
  58. // fast you are going now (present), This function will compute a reasonable
  59. // value for vc.  The value is never very far from the present speed, both
  60. // to attempt to stay within the power limit, and to maintain steering control.
  61. // You can adjust the resulting slip by changing "param".
  62. double wheel_speed(double goal, double present, double param)
  63. {
  64.    double ws;
  65.  
  66.    if(present > goal + 2 * param)  // if too fast,
  67.       ws = present - param;      // slow down.
  68.    else if(present < goal - 2 * param)  // if too slow,
  69.       ws = present + param;             // accelerate.
  70.    else                           // if quite close,
  71.       ws = (goal + present) / 2;      // approach desired speed gently.
  72.  
  73.    return ws;
  74. }
  75.  
  76. /* These two structures from CAR.H are repeated here as comments, because
  77.    the "driver" function receives situation as input and produces con_vec
  78.    as output.
  79. struct situation {       // a car's local situation as seen by the driver
  80.    double cur_rad;       // radius of inner wall of curve (0 means straight)
  81.    double cur_len;       // length of current track segment (angle if curve)
  82.    double to_lft;        // distance to left wall
  83.    double to_rgt;        // distance to right wall
  84.    double to_end;        // how far to end of current track seg. (angle or feet)
  85.    double v;             // the speed of the car, feet per second
  86.    double vn;            // component of v perpendicular to track direction
  87.    double nex_len;       // length of the next track segment (angle if curve)
  88.    double nex_rad;       // radius of inner wall of next segment (or 0)
  89.    double after_rad;     // radius of the segment after that one. (or 0)
  90.    double power_req;     // ratio: power requested by driver to maximum power
  91.    int dead_ahead;       // set when there is a car dead ahead, else 0
  92.    int backward;         // set if cars motion is opposed to track direction
  93.    rel_state* nearby;      // relative states of three cars in front of you
  94.    void* data_ptr;         // pointer to driver's scratchpad RAM area
  95. };
  96.  
  97. struct con_vec { double alpha, vc; };  // control vector, steering & throttle
  98. */
  99.  
  100. // The task of this function is to compute vc and alpha.  A high speed
  101. // car on a track is a little like the keel of a boat; if you set the keel
  102. // at a slight angle to the direction of the oncoming water, you get a large
  103. // force to the side.  That is how we corner the car.  The driver sets the
  104. // car at a slight angle with respect to its direction of motion, this
  105. // cause a force to the side, causing the path of the car to curve.  The
  106. // magnitude of the force is proportional to the angle (alpha) for very
  107. // small alpha, and when there is not much wheel spin.  The wheel spin
  108. // is controlled by vc, which is the rearward speed of the bottom of the
  109. // tire.  When going down the straight at a constant, moderate velocity,
  110. // then vc is equal to the speed of the car.  For acceleration, vc is
  111. // made a little greater than the speed.  For braking, it is made a little
  112. // less.  When accelerating, vc is limited by the power available.
  113. con_vec cntrlR(situation &s)
  114. {
  115.    const char name[] = "Rudy";        // This is the robot driver's name!
  116.    static int init_flag = 1;          // cleared by first call
  117.    double speed;                      // target speed for cornering, ft/sec
  118.    double speed_next;                 // target speed for next corner
  119.    con_vec result;                    // This is what is returned.
  120.    double width;                      // track width, feet
  121.    double redline;             // speed at which to begin braking on straight
  122.    double alpha, vc;           // components of result
  123.    static double alpha_inc = 0.0;  // alpha increment during passing maneuver
  124.    static int counting = 0;    // will be set and counting down when passing
  125.    params* p_ptr;
  126.  
  127.    // This paragraph has nothing to do with car control; it is just
  128.    // to identify the driver by copying its name to a global RAM area:
  129.    // This happens only on the very first call to this function
  130.  
  131.    p_ptr = (params*)s.data_ptr;     // point to the data area
  132.  
  133.    if(init_flag == 1)  {  // first time only, copy name:
  134.       my_name_is(name);        // copy the name string into the host program
  135.       init_flag = 0;
  136.       result.alpha = result.vc = 0;
  137.       return result;
  138.    }
  139.  
  140.    if(s.starting)  {  // one time only, set parameter values:
  141.       // These parameters may be adjusted to get better performance:
  142.       p_ptr->CORN_SPD_CON = 6.1; // determines how fast to take corners
  143.       p_ptr->STEER_GAIN = 1.1;     // servo gain, for staying in "lane"
  144.       p_ptr->STEER_DAMP = 0.8;     // servo damping, to prevent "weaving"
  145.       p_ptr->BRAK_ACCEL = 33.0; // acceleration during braking
  146.       p_ptr->END_CORNER = 2.3;// used to decide when to start leaving the corer
  147.       p_ptr->SLIP_LIM = 5.0;  // maximum wheel slip, ft/sec, in wheel_slip()
  148.       p_ptr->SLIP_CON = 500.0; //
  149.       p_ptr->BIAS = .09;        // cornering estimated alpha
  150.       p_ptr->NEAR_END = 2.2;   // widths from corner to start with BIAS
  151.       p_ptr->SHARP_TURN = .07; // change in alpha when attempting to pass
  152.       p_ptr->PASSING_TIME = 90;  // time to stay in passing maneuver, counts
  153.       result.alpha = 0.0;   result.vc = s.v + 40; // accelerate, full power!
  154.       return result;
  155.    }
  156.  
  157.   if(stuck(s.backward, s.v,s.vn, s.to_lft,s.to_rgt, &result.alpha,&result.vc))
  158.       return result;
  159.  
  160.    // Set alpha based on a servo-mechanism approach, trying to stay
  161.    // in the middle of the track, i.e., s.to_left equal to .5 * width:
  162.    width = s.to_lft + s.to_rgt;                        // find width of track
  163.    if(s.cur_rad > 0)  {
  164.       alpha = p_ptr->STEER_GAIN * (s.to_lft - .15 * width) / width;
  165.       alpha += p_ptr->BIAS;
  166.    }
  167.    else if(s.cur_rad < 0)  {
  168.       alpha = p_ptr->STEER_GAIN * (s.to_lft - .85 * width) / width;
  169.       alpha -= p_ptr->BIAS;
  170.    }
  171.    else {      // on straightaway
  172.      alpha = .2 * p_ptr->STEER_GAIN * (s.to_lft - .58 * width) / width;
  173.      if(s.to_end < p_ptr->NEAR_END * width)
  174.        if(s.nex_rad > 0)
  175.           alpha += p_ptr->BIAS;
  176.        else if(s.nex_rad < 0)
  177.           alpha -= p_ptr->BIAS;
  178.    }
  179.    alpha -= p_ptr->STEER_DAMP * s.vn / s.v;  // This is damping, to prevent oscillation
  180.  
  181.    // calculate target speeds for current corner and the next:
  182.    speed = corn_speed(s.cur_rad, p_ptr->CORN_SPD_CON);
  183.    speed_next = corn_speed(s.nex_rad, p_ptr->CORN_SPD_CON);
  184.  
  185.    // now set the tire speed, vc:
  186.    if(s.cur_rad == 0.0) {                  // If we are on a straightaway,
  187.       redline = sqrt(speed_next*speed_next + p_ptr->BRAK_ACCEL * 2.0*s.to_end);
  188.       if(s.v < redline)
  189.          vc = s.v + p_ptr->SLIP_CON / s.v;  // keep accellerating near full power
  190.       else                    // otherwise,
  191.          if(s.v <= 1.1 * speed_next)
  192.             vc = wheel_speed(speed_next, s.v, p_ptr->SLIP_LIM);
  193.          else
  194.             vc = 0.0;     // hard braking
  195.    }
  196.    else                              // If we're in the curve, maintain speed.
  197.       if(s.to_end*ABS(s.cur_rad) > p_ptr->END_CORNER * width *
  198.                                                        sqrt(s.v/speed_next)) {
  199.                   // if we are far from the next corner, stay at "speed".
  200.          vc = wheel_speed(speed, s.v, p_ptr->SLIP_LIM);
  201.          if(s.vn < .95 * speed)
  202.             alpha += s.cur_rad > 0 ? -p_ptr->BIAS : p_ptr->BIAS;  //remove bias
  203.       }   
  204.       else  {      // but when we near the next corner, adjust to "speed_next"
  205.          vc = wheel_speed(speed_next, s.v, p_ptr->SLIP_LIM);
  206.          alpha += s.cur_rad > 0 ? -p_ptr->BIAS : p_ptr->BIAS;     //remove bias
  207.       }
  208.  
  209.    // The passing maneuver:
  210.    if(s.dead_ahead & !counting)  {  // When first encountering the car ahead:
  211.       counting = p_ptr->PASSING_TIME;         // setup the timer,
  212.       if(rand() < RAND_MAX/2)        // choose a right or left maneuver:
  213.          alpha_inc = p_ptr->SHARP_TURN;
  214.       else
  215.          alpha_inc = -p_ptr->SHARP_TURN;
  216.    }
  217.    if(counting)  {                // If we are still in the passing maneuver,
  218.       alpha += alpha_inc;              // change alpha
  219.       --counting;                      // count down to zero
  220.    }
  221.  
  222.    result.vc = vc;   result.alpha = alpha;
  223.    return result;
  224. }
  225.